#include <LPC22xx.h>
#include "CANDriver.h"
#include "utility.h"
#include "DataFile.h"
#include <string.h>
#include <stdio.h>

#define CANBUFFERSIZE 50

extern SDataFile df;

// MUST BE IN ASCENDING ORDER!!!
// ID=0->CAN1, ID=1->CAN2
FULLCanID fcIds[2] =
{
	{ 0, 0x010, true, false },	// timestamp
	{ 0, 0x030, false, false },  // command
};

// single extended CAN IDs to receive
ExtendedCanID scIds[]=
{
	{0, 0x030 },
};

bool CANDriver::Init(int divider, int deviceType, int serial)
{
	// CAN1
	C1MOD = 0x01; // disable CAN	
	C1BTR = 0x001C0000 + divider; // set rate
	C1GSR = 0; // clear error counters
	// pins
	PINSEL1 |= 0x00040000;

    // CAN2
	C2MOD = 0x01; // disable CAN	
	C2BTR = 0x001C0000 + divider; // set rate
	C2GSR = 0; // clear error counters
	// pins
	PINSEL1 |= 0x00014000;

	// fill acceptance filter
	AFMR = 0x02; // begin AF update mode
	unsigned int* ptr = (unsigned int*)&AFRAM;
	unsigned tableSize = 0;
	// FULL CAN (11 bit IDs only!!!!)
	for(int i=0; i != sizeof(fcIds)/sizeof(FULLCanID); i=i+2 )
	{
		unsigned int entry = fcIds[i].ID;
		entry += (fcIds[i].CANCtrl << 13);
		if( fcIds[i].enabled == false ) entry = entry | 0x1000;
		if( fcIds[i].generateInt == true ) entry = entry | 0x0800;
		entry = entry<<16;
		entry += fcIds[i+1].ID;
		entry += (fcIds[i+1].CANCtrl << 13);
		if( fcIds[i+1].enabled == false ) entry = entry | 0x1000;
		if( fcIds[i+1].generateInt == true ) entry = entry | 0x0800;

		*ptr  = entry;
		ptr++;
		tableSize+=4;
	}

	SFF_sa = tableSize; 
	SFF_GRP_sa = tableSize; 
	EFF_sa = tableSize;

	// fill acceptance filter for single extended IDs
	for(int i=0; i != sizeof(scIds)/sizeof(ExtendedCanID); i++ )
	{
		unsigned int id = (scIds[i].ID<<16) + (serial<<8) + deviceType;
		unsigned int entry = id;
		entry += (scIds[i].CANCtrl << 29);	
		*ptr  = entry;
		ptr++;
		tableSize+=4;
	}

	EFF_GRP_sa = tableSize;
	ENDofTable= tableSize;

	// erase RAM
   	memset(ptr, 0, sizeof(fcIds)/sizeof(FULLCanID)*12);

	AFMR = 0x04; // enable Full CAN mode

	// enable ALL
	C1MOD = 0x00; // enable CAN	
	C2MOD = 0x00; // enable CAN	

	m_IsInRecovery[0] = false;
	m_IsInRecovery[1] = false;
	m_IsInRecovery[2] = false;
	m_IsInRecovery[3] = false;

	return false;
}

void CANDriver::GetErrorCounters(int CANNr, int& TEC, int& REC)
{
	int statusReg = 0;
	if( CANNr == 1 ) statusReg = C1GSR;		
	if( CANNr == 2 ) statusReg = C2GSR;		
	if( CANNr == 3 ) statusReg = C3GSR;		
	if( CANNr == 4 ) statusReg = C4GSR;		
	
	REC = (statusReg >> 16)	& 0x00FF;
	TEC = (statusReg >> 24)	& 0x00FF;
}

bool CANDriver::ReadDataFullCAN(int index, char* buffer)
{
	volatile unsigned char* tempPtr = (unsigned char*)&AFRAM;
	tempPtr = tempPtr + ENDofTable + (index*12);
	volatile unsigned int* ptr = (volatile unsigned int*)tempPtr;	
	unsigned int first = 0;

	do
	{
		first = *ptr;
	} while( (first & 0x03000000) == 0x01000000 );
	if( (first & 0x03000000) != 0x03000000 ) return false; // no new messages

	first = first & 0xFCFFFFFF; // clear SEM
	*ptr = first;

	unsigned int second = *(ptr+1);
	unsigned int third = *(ptr+2);
	memcpy(buffer, &second, 4);
	memcpy(buffer+4, &third, 4);
	first = *ptr;
	if( (first & 0x03000000) != 0x00000000 ) return false; // FAIL

	return true;
}

bool CANDriver::ReadData(int CANNr, char* buffer, int& id)
{
	if( CANNr == 1 )
	{
		// check new data
		unsigned int sr = C1SR;
		if( sr & 0x00000001 )
		{
			// data available, extract data
			int len = (C1RFS >> 16) && 0x0F;
			bool is29Bit = C1RFS >> 31;
	
		   	id = C1RID; // get ID
			*((unsigned int*)&buffer[0]) = C1RDA;
			*((unsigned int*)&buffer[4]) = C1RDB;
	
			// release buffer
			C1CMR = 0x00000004; // write RRB
	
			return true;
		}
	}
	if( CANNr == 2 )
	{
		// check new data
		unsigned int sr = C2SR;
		if( sr & 0x00000001 )
		{
			// data available, extract data
			int len = (C2RFS >> 16) && 0x0F;
			bool is29Bit = C2RFS >> 31;
	
		   	id = C2RID; // get ID
			*((unsigned int*)&buffer[0]) = C2RDA;
			*((unsigned int*)&buffer[4]) = C2RDB;
	
			// release buffer
			C2CMR = 0x00000004; // write RRB
	
			return true;
		}	
	}

	return false;
}

bool CANDriver::SendDataCAN1(bool extended, unsigned int ID, char* buffer )
{
	unsigned int sr = C1SR;	
							
	if( sr & 0x00000004 )
	{
		// buffer 1
		unsigned int fi = 0x00080000; // 8 bytes
		if( extended ) fi+=0x80000000; // 29bit identifier
		C1TFI1 = fi;

		C1TID1 = ID; // ID
		C1TDA1 = *((unsigned int*)&buffer[0]);
		C1TDB1 = *((unsigned int*)&buffer[4]);
		C1CMR = 0x0021; // send
	}
	else if( sr & 0x00000400 )
	{
		// buffer 2
		unsigned int fi = 0x00080000; // 8 bytes
		if( extended ) fi+=0x80000000; // 29bit identifier
		C1TFI2 = fi;

		C1TID2 = ID; // ID
		C1TDA2 = *((unsigned int*)&buffer[0]);
		C1TDB2 = *((unsigned int*)&buffer[4]);
		C1CMR = 0x0041; // send
	}
	else if( sr & 0x00040000 )
	{
		// buffer 3
		unsigned int fi = 0x00080000; // 8 bytes
		if( extended ) fi+=0x80000000; // 29bit identifier
		C1TFI3 = fi;

		C1TID3 = ID; // ID
		C1TDA3 = *((unsigned int*)&buffer[0]);
		C1TDB3 = *((unsigned int*)&buffer[4]);
		C1CMR = 0x0081; // send
	}
	else
	{
		return false; // FAILED
	}

	return true; // OK
}

bool CANDriver::SendDataCAN2(bool extended, unsigned int ID, char* buffer )
{
	unsigned int sr = C2SR;	
							
	if( sr & 0x00000004 )
	{
		// buffer 1
		unsigned int fi = 0x00080000; // 8 bytes
		if( extended ) fi+=0x80000000; // 29bit identifier
		C2TFI1 = fi;

		C2TID1 = ID; // ID
		C2TDA1 = *((unsigned int*)&buffer[0]);
		C2TDB1 = *((unsigned int*)&buffer[4]);
		C2CMR = 0x0021; // send
	}
	else if( sr & 0x00000400 )
	{
		// buffer 2
		unsigned int fi = 0x00080000; // 8 bytes
		if( extended ) fi+=0x80000000; // 29bit identifier
		C2TFI2 = fi;

		C2TID2 = ID; // ID
		C2TDA2 = *((unsigned int*)&buffer[0]);
		C2TDB2 = *((unsigned int*)&buffer[4]);
		C2CMR = 0x0041; // send
	}
	else if( sr & 0x00040000 )
	{
		// buffer 3
		unsigned int fi = 0x00080000; // 8 bytes
		if( extended ) fi+=0x80000000; // 29bit identifier
		C2TFI3 = fi;

		C2TID3 = ID; // ID
		C2TDA3 = *((unsigned int*)&buffer[0]);
		C2TDB3 = *((unsigned int*)&buffer[4]);
		C2CMR = 0x0081; // send
	}
	else
	{
		return false; // FAILED
	}

	return true; // OK
}

// TODO: call from periodic interrupt
void CANDriver::Update()
{
	// CAN1 - check for serious errors (busoff state)
	if( (C1GSR & 0x0080) && (m_IsInRecovery[0] == false) )
	{
		// reset
		C1MOD = 0x00; // enable CAN	
		m_IsInRecovery[0] = true;	
		df.CANErrorCount++;
	}
	if( m_IsInRecovery[0] == true )
	{
		// check error count (TEC must be zero to exit) and exit from recovery
		if( (C1GSR >> 24) == 0 ) m_IsInRecovery[0] = false;
	} 

	// CAN2 - check for serious errors (busoff state)
	if( (C2GSR & 0x0080) && (m_IsInRecovery[1] == false) )
	{
		// reset
		C2MOD = 0x00; // enable CAN	
		m_IsInRecovery[1] = true;	
		df.CANErrorCount++;
	}
	if( m_IsInRecovery[1] == true )
	{
		// check error count (TEC must be zero to exit) and exit from recovery
		if( (C2GSR >> 24) == 0 ) m_IsInRecovery[1] = false;
	} 

	// 3 output buffers, try to send 3 times
	for(int i=0; i!=3; i++ ) 
	{
		// send unsent packets
		if( m_CAN1PacketList.size() > 0 && m_IsInRecovery[0] == false )
		{
			SCANPacket p = m_CAN1PacketList[0];
			if( SendDataCAN1(p.extended, p.ID, p.buffer) == true )
			{
				m_CAN1PacketList.pop_front();	
			}
		}

		// send unsent packets
		if( m_CAN2PacketList.size() > 0 && m_IsInRecovery[1] == false )
		{
			SCANPacket p = m_CAN2PacketList[0];
			if( SendDataCAN2(p.extended, p.ID, p.buffer) == true )
			{
				m_CAN2PacketList.pop_front();	
			}
		}
	} 
}
bool CANDriver::BufferDataCAN(int canNr, bool extended, unsigned int ID, char* buffer)
{
	SCANPacket packet;
	packet.extended = extended;
	packet.ID = ID;
	memcpy(packet.buffer, buffer, 8);

	if( canNr == 1 )
	{
		if( m_CAN1PacketList.size() >= CANBUFFERSIZE ) return false; // overflow
		m_CAN1PacketList.push_back(packet);	
	}
	else if (canNr == 2 )
	{
		if( m_CAN2PacketList.size() >= CANBUFFERSIZE ) return false; // overflow
		m_CAN2PacketList.push_back(packet);	
	}

	Update(); // try to send now

	return true;
}
